#include "Brush.h"

using namespace libPortal;
using namespace boost;
using namespace std;
using namespace Eigen;

namespace libPortal
{

  BSPPlane::BSPPlane(Eigen::Hyperplane<double, 3> planeEq) :
    planeShape(planeEq)
  {}
  
  bool isRectangular(const BSPBrush3D &b, Transform<double, 3> &transform)
  {
    if(b.planes.size()!=6)
      return false;  //all rectangular prisms have 6 sides.

    Vector3d localAxis[3];
    Vector3d dists;
    Vector3d aveVec(0,0,0);
    vector<shared_ptr<BSPPlane> >::const_iterator outerIter, outerEnd;
    vector<shared_ptr<BSPPlane> >::const_iterator innerIter, innerEnd;
    outerIter = b.planes.begin();
    outerEnd = b.planes.end();
    innerEnd = b.planes.end();
    unsigned char parallelCount = 0;
    for(;outerIter!=outerEnd;outerIter++)
      {
	Vector3d v1((*outerIter)->planeShape.plane.normal());
	double d1 = (*outerIter)->planeShape.plane.offset();
	v1.normalize();
	innerIter = outerIter;
	for(innerIter++;innerIter!=innerEnd;innerIter++)
	  {
	    Vector3d v2 = (*innerIter)->planeShape.plane.normal();
	    double d2 = (*innerIter)->planeShape.plane.offset();
	    v2.normalize();
	    double dot = v1.dot(v2);
	    if(
	       dot<-0.99999 ||
	       (dot>0.99999 && dot<1.00001)
	       ) //found a parallel plane pair.
	      {
		if(parallelCount>=3)
		  return false;
		localAxis[parallelCount] = v1;
		dists[parallelCount] = abs(d1+d2);
		aveVec += ((v1*-d1)+(v2*-d2))/2.0;
		parallelCount++;
	      }
	  }
      }
    if(parallelCount==3)
      {
	Matrix3d rot;
	transform.setIdentity();
	//Setup rotation
	rot << localAxis[0][0], localAxis[1][0], localAxis[2][0],
               localAxis[0][1], localAxis[1][1], localAxis[2][1],
	       localAxis[0][2], localAxis[1][2], localAxis[2][2];
	  

	//Setup translation
	Vector3d center = aveVec;
	transform.translate(center);
	transform.scale(dists);
	transform.rotate(rot);
	//Setup scale.
	
	return true;
      }
    else
      return false;
  }
}
